// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GraphOptimization.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Localization
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Reflection;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// This class performs graph optimization for the purpose of correcting for loop closure.
    /// The algorithms are described in the paper, $B. Peasley and S. Birchfield, Fast and accurate PoseSLAM by combining relative and global state spaces, ICRA 2014 (submitted)$.
    /// The code assumes all data are in a right-hand coordinate system, with positive angles defined counterclockwise.  But the definition of theta=0 is irrelevant.
    /// </summary>
    public class GraphOptimization
    {
        /// <summary>
        /// Gets or sets a value indicating whether to enable logging.
        /// </summary>
        public static bool EnableLogging { get; set; }

        /// <summary>
        /// Gets or sets the name of directory in which to save logging information.
        /// </summary>
        public static string LoggingDirectory { get; set; }

        /// <summary>
        /// Optimizes the graph.  This is the entry point.
        /// This method runs (rPOReSS) -> (POReSS) -> (Graph-Seidel), where the parentheses indicate that each step is optional.
        /// The order of the steps is fixed.
        /// </summary>
        /// <param name="initialPoses">poses $\bfp_0, ..., \bfp_n$</param>
        /// <param name="runRotationalPoress">whether to run the rPOReSS algorithm (considering only orientation)</param>
        /// <param name="runFullPoress">whether to run the full POReSS algorithm</param>
        /// <param name="runGraphSeidel">whether to run Graph-Seidel</param>
        /// <returns>optimized poses $\bfp_0, ..., \bfp_n$</returns>
        public static Pose2D[] OptimizeGraphForPoseSlamSimpleLoop(Pose2D[] initialPoses, bool runRotationalPoress, bool runFullPoress, bool runGraphSeidel)
        {
            RelativePoseEdge[] edges = ConvertPosesToEdgesSimpleLoopClosure(initialPoses);
            Pose2D[] states = ConvertPosesToRelativeStates(initialPoses);
            if (runRotationalPoress)
            {
                Pose2D[] tmpStates = RunRotationalPoress(edges, states);
                states = (Pose2D[])tmpStates.Clone();
            }

            if (runFullPoress)
            {
                Pose2D[] tmpStates = RunPoress(edges, states);
                states = (Pose2D[])tmpStates.Clone();
            }

            PoseEdgeGss[] edgesGss = ConvertPosesToEdgesGssSimpleLoopClosure(initialPoses);
            Pose2D[] poses = ConvertRelativeStatesToPoses(states);
            if (runGraphSeidel)
            {
                Pose2D[] tmpPoses = RunGraphSeidel(edgesGss, poses, edges);
                poses = (Pose2D[])tmpPoses.Clone();
            }

            return poses;
        }

        /// <summary>
        /// Save poses to file.
        /// </summary>
        /// <param name="poses">the poses</param>
        /// <param name="filename">the filename</param>
        internal static void SavePosesToFile(Pose2D[] poses, string filename)
        {
            System.IO.FileStream fs = new System.IO.FileStream(filename, System.IO.FileMode.Create, System.IO.FileAccess.Write);
            System.IO.StreamWriter writer = new System.IO.StreamWriter(fs);

            for (int i = 0; i < poses.Length; i++)
            {
                Pose2D pose = poses[i];
                writer.WriteLine("{0} {1} {2}", pose.X, pose.Y, pose.Heading);
            }

            writer.Close();
        }

        /// <summary>
        /// Converts 2D poses to states in a relative state space (RSS).  
        /// </summary>
        /// <param name="poses">poses $\bfp_0, ..., \bfp_n$.  These are zero-based indexed just as in the paper, with poses[0] defining the coordinate system, and poses[i] equal to $\bfp_i$.</param>
        /// <returns>relative states $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</returns>
        internal static Pose2D[] ConvertPosesToRelativeStates(Pose2D[] poses)
        {
            // To understand this code, see Eqns. (12)-(14) in the paper.
            int n = poses.Length - 1;  // 'n' is the number of states
            Pose2D[] states = new Pose2D[n];
            for (int i = 1; i <= n; i++)
            {
                Pose2D poseDiff = poses[i] - poses[i - 1];  // \bfp_i - \bfp_{i-1}
                double dx = poseDiff.X;
                double dy = poseDiff.Y;
                double dt = poseDiff.Heading;
                double thetaSubiMinusone = poses[i - 1].Heading;  // \theta_{i-1}
                double c = Math.Cos(thetaSubiMinusone);  // \cos(\theta_{i-1})
                double s = Math.Sin(thetaSubiMinusone);  // \sin(\theta_{i-1})
                states[i - 1].X = c * dx + s * dy;
                states[i - 1].Y = -s * dx + c * dy;
                states[i - 1].Heading = dt;
            }

            return states;
        }

        /// <summary>
        /// Converts an array of consecutive poses into graph edges, while adding an extra edge to connect the first and last poses.  
        /// </summary>
        /// <param name="poses">consecutive poses $\bfp_0, ..., \bfp_n$</param>
        /// <returns>array of edges between $\bfp_i$ and $\bfp_{i-1}$ for $i=1, ..., n$, along with the edge between $\bfp_n$ and $\bfp_0$</returns>
        internal static RelativePoseEdge[] ConvertPosesToEdgesSimpleLoopClosure(Pose2D[] poses)
        {
            // Add edges between consecutive poses
            int n = poses.Length - 1;  // 'n' is the number of states
            RelativePoseEdge[] edges = new RelativePoseEdge[n + 1];
            for (int i = 1; i <= n; i++)
            {
                edges[i - 1] = ConvertPosePairToEdge(i - 1, poses[i - 1], i, poses[i]);
            }

            // Add additional edge for loop closure
            edges[n] = new RelativePoseEdge(0, n, new Pose2D(0, 0, 0));

            return edges;
        }

        /// <summary>
        /// Converts an array of consecutive poses into graph edges, while adding an extra edge to connect the first and last poses.  
        /// Graph edges are in a global state space.
        /// </summary>
        /// <param name="poses">consecutive poses $\bfp_0, ..., \bfp_n$</param>
        /// <returns>array of edges between $\bfp_i$ and $\bfp_{i-1}$ for $i=1, ..., n$, along with the edge between $\bfp_n$ and $\bfp_0$</returns>
        internal static PoseEdgeGss[] ConvertPosesToEdgesGssSimpleLoopClosure(Pose2D[] poses)
        {
            // Add edges between consecutive poses
            int n = poses.Length - 1;  // 'n' is the number of states
            PoseEdgeGss[] edges = new PoseEdgeGss[n + 1];
            for (int i = 1; i <= n; i++)
            {
                edges[i - 1] = ConvertPosePairToEdgeGss(i - 1, poses[i - 1], i, poses[i]);
            }

            // Add additional edge for loop closure
            edges[n] = new PoseEdgeGss(0, n, new Vector4(0, 0, 0, 0));

            return edges;
        }

        /// <summary>
        /// Computes the root-mean-square (RMS) error associated with a sequence of states in a relative state space.
        /// This should yield the exact same result as $ComputeErrorGss()$.
        /// </summary>
        /// <param name="edges">Edges in the graph</param>
        /// <param name="states">States (in relative state space) $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <returns>Error, which is the L2-norm of the vector of residuals.  Note that the covariance matrix is not taken into account.</returns>
        internal static double ComputeError(RelativePoseEdge[] edges, Pose2D[] states)
        {
            if (edges.Length == 0)
            {
                throw new ArgumentException("'edges' cannot be empty");
            }

            double error = 0;
#if DEBUG
            double[] errorArr = new double[edges.Length];
#endif
            for (int j = 0; j < edges.Length; j++)
            {
                RelativePoseEdge edge = edges[j];
                int a = edge.A;
                int b = edge.B;
                Pose2D poseAB = ConvertRelativeStatesToRelativePose(states, a, b);
                Pose2D residual = edge.Delta - poseAB;
                residual.Heading = Pose2D.ClampAngle(residual.Heading);
                double errorAB = residual.X * residual.X + residual.Y * residual.Y + residual.Heading * residual.Heading;
#if DEBUG
                errorArr[j] = errorAB;
#endif
                error += errorAB;
            }

            return Math.Sqrt(error / edges.Length);
        }

        /// <summary>
        /// Computes the root-mean-square (RMS) error associated with a sequence of states in a global state space (GSS).
        /// This should yield the exact same result as ComputeError().
        /// </summary>
        /// <param name="edges">Edges in the graph</param>
        /// <param name="poses">poses $\bfp_0, ..., \bfp_n$</param>
        /// <returns>Error, which is the L2-norm of the vector of residuals.  Note that the covariance matrix is not taken into account.</returns>
        internal static double ComputeErrorGss(RelativePoseEdge[] edges, Pose2D[] poses)
        {
            if (edges.Length == 0)
            {
                throw new ArgumentException("'edges' cannot be empty");
            }

            double error = 0;
#if DEBUG
            double[] errorArr = new double[edges.Length];
#endif
            for (int j = 0; j < edges.Length; j++)
            {
                RelativePoseEdge edge = edges[j];
                int a = edge.A;
                int b = edge.B;
                Pose2D poseA = poses[a];
                Pose2D poseB = poses[b];
                Pose2D poseDiff = poseB - poseA;
                double ca = Math.Cos(poseA.Heading);
                double sa = Math.Sin(poseA.Heading);
                double relX = ca * poseDiff.X + sa * poseDiff.Y;
                double relY = -sa * poseDiff.X + ca * poseDiff.Y;
                Pose2D poseAB = new Pose2D(relX, relY, poseDiff.Heading);
                Pose2D residual = edge.Delta - poseAB;
                residual.Heading = Pose2D.ClampAngle(residual.Heading);
                double errorAB = residual.X * residual.X + residual.Y * residual.Y + residual.Heading * residual.Heading;
#if DEBUG
                errorArr[j] = errorAB;
#endif
                error += errorAB;
            }

            return Math.Sqrt(error / edges.Length);
        }

        /// <summary>
        /// Computes the root-mean-square (RMS) error associated with a sequence of states in a global state space (GSS).
        /// This should yield the exact same result as ComputeError().
        /// </summary>
        /// <param name="edges">Edges in the graph</param>
        /// <param name="posesGss">poses $\bfp_0, ..., \bfp_n$</param>
        /// <returns>Error, which is the L2-norm of the vector of residuals.  Note that the covariance matrix is not taken into account.</returns>
        internal static double ComputeErrorGss(RelativePoseEdge[] edges, Vector4[] posesGss)
        {
            Pose2D[] poses = ConvertPoseGssToPoseArray(posesGss);
            return ComputeErrorGss(edges, poses);
        }

        /// <summary>
        /// Runs the rPOReSS algorithm.
        /// </summary>
        /// <param name="edges">Edges $\delta_{ab}$ of the graph</param>
        /// <param name="initialStates">Initial states (in relative state space) $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <returns>Optimized states</returns>
        private static Pose2D[] RunRotationalPoress(RelativePoseEdge[] edges, Pose2D[] initialStates)
        {
            Pose2D[] states = (Pose2D[])initialStates.Clone();

            // precompute M is not needed in rPoress

            // parameters
            double lambda = 0.9;  // learning rate, anything between 0.5 and 1.0 will work fine, with larger numbers converging faster
            const int MaxIterations = 20;  // max iterations, usually only 3 iterations are needed or so, depending on lambda
            const double MinimumMeaningfulErrorReduction = 0.001;  // when the error reduces by less than this amount, then there is no reason to continue; units are a combination of meters and radians
            const double LambdaReductionFactorForConsecutiveEdges = 0.1;  // we weight the loop closing edges more than the consecutive edges; this number must be small, < 0.1
            const double SmallestUsefulLambda = 0.1;  // minimal learning rate; below this we are wasting computation for no reason

            // optimize
            double bestError = ComputeError(edges, states);
            TraceOut.Info(TraceContexts.Nav, "GraphOptimization:  rPOReSS iteration {0}  error {1}", 1, bestError);
            Pose2D[] bestStates = (Pose2D[])states.Clone();
            int iteration = 1;
            while (lambda > SmallestUsefulLambda && iteration <= MaxIterations)
            {
                if (EnableLogging)
                {
                    string filename = LoggingDirectory + "/" + string.Format("poses{0}.txt", iteration);
                    Pose2D[] poses = ConvertRelativeStatesToPoses(states);
                    SavePosesToFile(poses, filename);
                }

                for (int j = 0; j < edges.Length; j++)
                {
                    RelativePoseEdge edge = edges[j];
                    int a = edge.A;
                    int b = edge.B;
                    if (b < a + 1)
                    {
                        throw new Exception("The graph edges A and B are out of order.  B should be >= A + 1.");
                    }

                    Pose2D poseAB = ConvertRelativeStatesToRelativePose(states, a, b);
                    Pose2D residual = edge.Delta - poseAB;
                    residual.Heading = Pose2D.ClampAngle(residual.Heading);
                    if (b == a + 1)
                    {
                        states[b - 1].Heading += lambda * LambdaReductionFactorForConsecutiveEdges * residual.Heading;
                    }
                    else
                    {
                        for (int i = a + 1; i <= b; i++)
                        {
                            states[i - 1].Heading += (lambda / (b - a)) * residual.Heading;
                        }
                    }
                }

                double error = ComputeError(edges, states);
                TraceOut.Info(TraceContexts.Nav, "GraphOptimization:  rPOReSS iteration {0}  lambda {2} error {1} bestError {3}", iteration + 1, error, lambda, bestError);

                if (error < bestError)
                {
                    double reduction = bestError - error;

                    // accept these states as the best seen so far
                    bestError = error;
                    bestStates = (Pose2D[])states.Clone();

                    if (reduction < MinimumMeaningfulErrorReduction)
                    {
                        break;
                    }
                }
                else
                {
                    // revert to the best states seen so far
                    states = (Pose2D[])bestStates.Clone();
                    break;
                }

                iteration++;
            }

            if (EnableLogging)
            {
                string filename = LoggingDirectory + @"\" + string.Format("poses{0}.txt", iteration);
                Pose2D[] poses = ConvertRelativeStatesToPoses(states);
                SavePosesToFile(poses, filename);
            }

            if (EnableLogging)
            {
                // write final best poses (should be the same as the final poses above unless the code has been modified)
                string filename = LoggingDirectory + @"\" + string.Format("poses{0}.txt", iteration + 1);
                Pose2D[] poses = ConvertRelativeStatesToPoses(bestStates);
                SavePosesToFile(poses, filename);
            }

            return bestStates;
        }

        /// <summary>
        /// Computes alpha and beta
        /// </summary>
        /// <param name="states">states (in relative state space) $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <param name="a">first node $a$</param>
        /// <param name="b">second node $b$</param>
        /// <param name="i">intermediate node $i$</param>
        /// <param name="alphaABI">output alpha, ${^a_b}\alpha_i$</param>
        /// <param name="betaABI">output beta, ${^a_b}\beta_i$</param>
        private static void ComputeAlphaBeta(Pose2D[] states, int a, int b, int i, out double alphaABI, out double betaABI)
        {
            // To understand this code, see Eqn. (22) in the paper.
            alphaABI = 0;
            betaABI = 0;
            for (int k = i; k <= b; k++)
            {
                double thetaAkminusone = ComputeThetaAB(states, a, k - 1);
                double s = Math.Sin(thetaAkminusone);
                double c = Math.Cos(thetaAkminusone);
                double xkprime = states[k - 1].X;
                double ykprime = states[k - 1].Y;
                alphaABI += -xkprime * s - ykprime * c;
                betaABI += xkprime * c - ykprime * s;
            }
        }

        /// <summary>
        /// Runs the POReSS algorithm.
        /// </summary>
        /// <param name="edges">Edges $\delta_{ab}$ of the graph</param>
        /// <param name="initialStates">Initial states (in relative state space) $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <returns>Optimized states</returns>
        private static Pose2D[] RunPoress(RelativePoseEdge[] edges, Pose2D[] initialStates)
        {
            Pose2D[] states = (Pose2D[])initialStates.Clone();
            int n = initialStates.Length;

            // precompute M
            double[] m = new double[3 * n];
            for (int j = 0; j < edges.Length; j++)
            {
                RelativePoseEdge edge = edges[j];
                int a = edge.A;
                int b = edge.B;
                if (b < a + 1)
                {
                    throw new Exception("The graph edges A and B are out of order.  B should be >= A + 1.");
                }

                if (b == a + 1)
                {
                    // Here we assume that \Omega_{ab} is the identity matrix
                    int index = a;
                    m[index] += 1;
                    m[index + 1] += 1;
                    m[index + 2] += 1;
                }
                else
                {
                    for (int i = a + 1; i <= b; i++)
                    {
                        // Here we assume that \Omega_{ai} is the identity matrix
                        double alpha, beta;
                        ComputeAlphaBeta(states, a, b, i, out alpha, out beta);
                        int index = i - 1;
                        m[3 * index] += 1;
                        m[3 * index + 1] += 1;
                        m[3 * index + 2] += alpha * alpha + beta * beta + 1;
                    }
                }
            }

            // parameters
            double lambda = 0.9;  // learning rate, anything between 0.5 and 1.0 will work fine, with larger numbers converging faster
            const int MaxIterations = 20;  // max iterations, usually only 3 iterations are needed or so, depending on lambda
            const double MinimumMeaningfulErrorReduction = 0.001;  // when the error reduces by less than this amount, then there is no reason to continue; units are a combination of meters and radians
            const double LambdaReductionFactorForConsecutiveEdges = 0.1;  // we weight the loop closing edges more than the consecutive edges; this number must be small, < 0.1
            const double SmallestUsefulLambda = 0.1;  // minimal learning rate; below this we are wasting computation for no reason

            // optimize
            double bestError = ComputeError(edges, states);
            TraceOut.Info(TraceContexts.Nav, "GraphOptimization:  POReSS iteration {0}  error {1}\n", 1, bestError);
            Pose2D[] bestStates = (Pose2D[])states.Clone();
            int iteration = 1;
            while (lambda > SmallestUsefulLambda && iteration <= MaxIterations)
            {
                if (EnableLogging)
                {
                    string filename = LoggingDirectory + "/" + string.Format("poses{0}.txt", iteration);
                    Pose2D[] poses = ConvertRelativeStatesToPoses(states);
                    SavePosesToFile(poses, filename);
                }

                for (int j = 0; j < edges.Length; j++)
                {
                    RelativePoseEdge edge = edges[j];
                    int a = edge.A;
                    int b = edge.B;
                    if (b < a + 1)
                    {
                        throw new Exception("The graph edges A and B are out of order.  B should be >= A + 1.");
                    }

                    Pose2D poseAB = ConvertRelativeStatesToRelativePose(states, a, b);
                    Pose2D residual = edge.Delta - poseAB;
                    residual.Heading = Pose2D.ClampAngle(residual.Heading);
                    if (b == a + 1)
                    {
                        double lambdaFactor = lambda * LambdaReductionFactorForConsecutiveEdges;
                        states[b - 1].X += lambdaFactor * residual.X;
                        states[b - 1].Y += lambdaFactor * LambdaReductionFactorForConsecutiveEdges * residual.Y;
                        states[b - 1].Heading += lambdaFactor * LambdaReductionFactorForConsecutiveEdges * residual.Heading;
                    }
                    else
                    {
                        // Here we assume that \Omega_{ab} is the identity matrix
                        for (int i = a + 1; i <= b; i++)
                        {
                            double alpha, beta;
                            ComputeAlphaBeta(states, a, b, i, out alpha, out beta);
                            double thetaAiminusone = ComputeThetaAB(states, a, i - 1);
                            double s = Math.Sin(thetaAiminusone);
                            double c = Math.Cos(thetaAiminusone);
                            Pose2D delta = new Pose2D(
                                c * residual.X - s * residual.Y + alpha * residual.Heading,
                                s * residual.X + c * residual.Y + beta * residual.Heading,
                                residual.Heading);
                            double lambdaAB = lambda / (b - a);
                            int index = i - 1;
                            states[i - 1].X += lambdaAB * delta.X / m[3 * index];
                            states[i - 1].Y += lambdaAB * delta.Y / m[3 * index + 1];
                            states[i - 1].Heading += lambdaAB * delta.Heading / m[3 * index + 2];
                        }
                    }
                }

                double error = ComputeError(edges, states);
                TraceOut.Info(TraceContexts.Nav, "GraphOptimization:  POReSS iteration {0}  lambda {2} error {1} bestError {3}\n", iteration + 1, error, lambda, bestError);

                if (error < bestError)
                {
                    double reduction = bestError - error;

                    // accept these states as the best seen so far
                    bestError = error;
                    bestStates = (Pose2D[])states.Clone();

                    if (reduction < MinimumMeaningfulErrorReduction)
                    {
                        break;
                    }
                }
                else
                {
                    // revert to the best states seen so far
                    states = (Pose2D[])bestStates.Clone();
                    break;
                }

                iteration++;
            }

            if (EnableLogging)
            {
                string filename = LoggingDirectory + "/" + string.Format("poses{0}.txt", iteration);
                Pose2D[] poses = ConvertRelativeStatesToPoses(states);
                SavePosesToFile(poses, filename);
            }

            if (EnableLogging)
            {
                // write final best poses (should be the same as the final poses above unless the code has been modified)
                string filename = LoggingDirectory + "/" + string.Format("poses{0}.txt", iteration + 1);
                Pose2D[] poses = ConvertRelativeStatesToPoses(states);
                SavePosesToFile(poses, filename);
            }

            return bestStates;
        }

        /// <summary>
        /// Runs the Graph-Seidel algorithm.
        /// </summary>
        /// <param name="edgesGss">Edges $\delta_{ab}$ of the graph in GSS</param>
        /// <param name="initialPoses">Initial poses $\bfp_0, ..., \bfp_n$, according to the notation in the paper.  Except for the initial pose, these are identical to the states (in global state space).</param>
        /// <param name="graphEdges">Edges $\delta_{ab}$ of the graph in RSS (used only for computing the residual)</param>
        /// <returns>Optimized poses</returns>
        private static Pose2D[] RunGraphSeidel(PoseEdgeGss[] edgesGss, Pose2D[] initialPoses, RelativePoseEdge[] graphEdges)
        {
            int nposes = initialPoses.Length;  // number of poses
            int n = nposes - 1;  // number of states
            Vector4[] v = new Vector4[nposes];  // This is 'v' in the paper, i.e., the righthand side of the equation Ax=v to be solved.  v[0] is not used.

            // Convert poses to GSS
            Vector4[] poses = ConvertPoseToPoseGssArray(initialPoses);

            double bestError = ComputeErrorGss(graphEdges, poses);
            TraceOut.Info(TraceContexts.Nav, "GraphOptimization:  GraphSeidel iteration {0}  error {1}", 1, bestError);

            // Precompute edge lists (one for each pose)
            List<PoseEdgeGss>[] edgesIn = new List<PoseEdgeGss>[nposes];
            List<PoseEdgeGss>[] edgesOut = new List<PoseEdgeGss>[nposes];
            for (int i = 0; i < edgesIn.Length; i++)
            {
                edgesIn[i] = new List<PoseEdgeGss>();
                edgesOut[i] = new List<PoseEdgeGss>();
            }

            for (int j = 0; j < edgesGss.Length; j++)
            {
                PoseEdgeGss edge = edgesGss[j];
                edgesIn[edge.B].Add(edge);
                edgesOut[edge.A].Add(edge);
            }

            const int MaxIterations = 200;  // max iterations, usually needed 100 iterations or so, since Gauss-Seidel converges more slowly than POReSS
            const double MinimumMeaningfulChange = 0.001;  // when the average L1 norm of the change in state is less than this amount, then there is no reason to continue; units are a combination of meters and radians
            const double Omega = 1.9;  // Successive over-relaxation (SOR) parameter.  Omega=1 runs Gauss-Seidel.  Must be > 0 and < 2.  Recommended to be close to 2.
            int iteration = 1;
            Vector4[] bestPoses = (Vector4[])poses.Clone();
            while (iteration <= MaxIterations)
            {
                if (EnableLogging)
                {
                    string filename = LoggingDirectory + "/" + string.Format("posesgs{0}.txt", iteration);
                    Pose2D[] p = ConvertPoseGssToPoseArray(bestPoses);
                    SavePosesToFile(p, filename);
                }

                // Compute 'v', i.e., the righthand side of the equation Ax=v to be solved
                for (int i = 1; i <= n; i++)
                {
                    v[i].X = 0;
                    v[i].Y = 0;
                    v[i].Z = 0;
                    v[i].W = 0;
                    for (int j = 0; j < edgesIn[i].Count; j++)
                    {
                        PoseEdgeGss edge = edgesIn[i][j];
                        v[i] += edge.Delta;
                    }

                    for (int j = 0; j < edgesOut[i].Count; j++)
                    {
                        PoseEdgeGss edge = edgesOut[i][j];
                        v[i] -= edge.Delta;
                    }
                }

                // Solve Ax=v using Gauss-Seidel (with successive over-relaxation) to update states
                double amountChanged = 0;
                for (int i = 1; i <= n; i++)
                {
                    // Solve Ax=v for the ith state $\bfx_i$
                    Vector4 w = new Vector4(0, 0, 0, 0);
                    for (int j = 0; j < edgesIn[i].Count; j++)
                    {
                        PoseEdgeGss edge = edgesIn[i][j];
                        w += poses[edge.A];
                    }

                    for (int j = 0; j < edgesOut[i].Count; j++)
                    {
                        PoseEdgeGss edge = edgesOut[i][j];
                        w += poses[edge.B];
                    }

                    int numEdges = edgesIn[i].Count + edgesOut[i].Count;
                    Vector4 oldStateGss = poses[i];
                    Vector4 newStateGss = (v[i] + w) / numEdges;

                    // Update state using SOR
                    poses[i] = poses[i] * (1 - Omega) + newStateGss * Omega;

                    // Determine how much state changed
                    amountChanged += Vector4.ComputeL2Norm(poses[i] - oldStateGss);
                }

                amountChanged /= n;

                if (amountChanged < MinimumMeaningfulChange)
                {
                    break;
                }

                double error = ComputeErrorGss(graphEdges, poses);
                TraceOut.Info(TraceContexts.Nav, "GraphOptimization:  GraphSeidel iteration {0}  error {1} bestError {2}", iteration + 1, error, bestError);

                if (error < bestError)
                {
                    bestPoses = (Vector4[])poses.Clone();
                    bestError = error;
                }

                iteration++;
            }

            // Convert poses from GSS
            Pose2D[] optimizedPoses = ConvertPoseGssToPoseArray(bestPoses);

            if (EnableLogging)
            {
                string filename = LoggingDirectory + "/" + string.Format("posesgs{0}.txt", iteration);
                SavePosesToFile(optimizedPoses, filename);
            }

            return optimizedPoses;
        }

        /// <summary>
        /// Converts states in a relative state space (RSS) to 2D poses.
        /// </summary>
        /// <param name="states">relative states $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <param name="firstPose">first pose $\bfp_0$, which indicates the coordinate system</param>
        /// <returns>poses $\bfp_0, ..., \bfp_n$.  These are zero-based indexed just as in the paper, with poses[0] defining the coordinate system, and poses[i] equal to $\bfp_i$.</returns>
        private static Pose2D[] ConvertRelativeStatesToPoses(Pose2D[] states, Pose2D firstPose)
        {
            // To understand this code, see Eqns. (12)-(14) in the paper.
            int n = states.Length;  // 'n' is the number of states
            Pose2D[] poses = new Pose2D[n + 1];
            poses[0] = firstPose;
            for (int i = 1; i <= n; i++)
            {
                double xPrimeSubi = states[i - 1].X;  // x'_i
                double yPrimeSubi = states[i - 1].Y;  // y'_i
                double thetaPrimeSubi = states[i - 1].Heading;  // \theta'_i
                double thetaSubiMinusone = poses[i - 1].Heading;  // \theta_{i-1}
                double c = Math.Cos(thetaSubiMinusone);  // \cos(\theta_{i-1})
                double s = Math.Sin(thetaSubiMinusone);  // \sin(\theta_{i-1})
                double deltaX = c * xPrimeSubi - s * yPrimeSubi;
                double deltaY = s * xPrimeSubi + c * yPrimeSubi;
                double deltaTheta = thetaPrimeSubi;
                poses[i] = poses[i - 1] + new Pose2D(deltaX, deltaY, deltaTheta);
            }

            return poses;
        }

        /// <summary>
        /// Converts states in a relative state space (RSS) to 2D poses.
        /// </summary>
        /// <param name="states">relative states $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <returns>poses $\bfp_0, ..., \bfp_n$.  These are zero-based indexed just as in the paper, with poses[0] defining the coordinate system, and poses[i] equal to $\bfp_i$.</returns>
        private static Pose2D[] ConvertRelativeStatesToPoses(Pose2D[] states)
        {
            return ConvertRelativeStatesToPoses(states, new Pose2D(0, 0, 0));
        }

        /// <summary>
        /// Computes the relative angle between two states.
        /// </summary>
        /// <param name="states">relative states $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <param name="a">index $a$ of the first state</param>
        /// <param name="b">index $b$ of the first state</param>
        /// <returns>the angle $^a\theta_b$ between states $\bfx_a$ and $\bfx_b$</returns>
        private static double ComputeThetaAB(Pose2D[] states, int a, int b)
        {
            // To understand this code, see Eqn. (16) in the paper.
            double thetaAB = 0;
            for (int k = a + 1; k <= b; k++)
            {
                thetaAB += states[k - 1].Heading;
            }

            return thetaAB;  // $^a\theta_b$
        }

        /// <summary>
        /// Computes the relative pose between two states in a relative state space (RSS).
        /// </summary>
        /// <param name="states">relative states $\bfx_1, ..., \bfx_n$, according to the notation in the paper.  Since these are zero-based indexed in the code, states[i-1] is actually $\bfx_i$.</param>
        /// <param name="a">index $a$ of the first pose.</param>
        /// <param name="b">index $b$ of the second pose.</param>
        /// <returns>relative pose $^a\bfp_b$ between pose $\bfp_a$ and $\bfp_b$.</returns>
        private static Pose2D ConvertRelativeStatesToRelativePose(Pose2D[] states, int a, int b)
        {
            // To understand this code, see Eqns. (15)-(16) in the paper.
            int n = states.Length;  // 'n' is the number of states
            Pose2D poseAB = new Pose2D(0, 0, 0);
            for (int k = a + 1; k <= b; k++)
            {
                double xPrimeSubk = states[k - 1].X;  // x'_k
                double yPrimeSubk = states[k - 1].Y;  // y'_k
                double thetaPrimeSubk = states[k - 1].Heading;  // \theta'_k
                double thetaASubkMinusone = ComputeThetaAB(states, a, k - 1);  // ^a\theta_{k-1}
                double c = Math.Cos(thetaASubkMinusone);  // \cos(^a\theta_{k-1})
                double s = Math.Sin(thetaASubkMinusone);  // \sin(^a\theta_{k-1})
                double deltaX = c * xPrimeSubk - s * yPrimeSubk;
                double deltaY = s * xPrimeSubk + c * yPrimeSubk;
                double deltaTheta = thetaPrimeSubk;
                poseAB += new Pose2D(deltaX, deltaY, deltaTheta);
            }

            return poseAB;
        }

        /// <summary>
        /// Converts two poses (not necessarily consecutive) into the relative pose between them.
        /// </summary>
        /// <param name="pose1">first pose $\bfp_a$, according to the notation in the paper.</param>
        /// <param name="pose2">second pose $\bfp_b$.</param>
        /// <returns>relative pose between the poses, that is, pose $b$ in frame $a$, $^a\bfp_b$.</returns>
        private static Pose2D ConvertPosePairToRelativePose(Pose2D pose1, Pose2D pose2)
        {
            // To understand this code, see Eqns. (15)-(16) in the paper.
            Pose2D poseDiff = pose2 - pose1;  // \bfp_b - \bfp_a
            double theta1 = pose1.Heading;  // \theta_a
            double c = Math.Cos(theta1);  // \cos(\theta_a)
            double s = Math.Sin(theta1);  // \sin(\theta_a)
            double dx = poseDiff.X;
            double dy = poseDiff.Y;
            double dxr = c * dx + s * dy;
            double dyr = -s * dx + c * dy;
            double dt = poseDiff.Heading;
            return new Pose2D(dxr, dyr, dt);
        }

        /// <summary>
        /// Convert pose to the global state space (GSS) by representing the heading as its sine and cosine.
        /// </summary>
        /// <param name="pose">Pose to be converted.</param>
        /// <returns>Pose with heading represented as its sine and cosine.</returns>
        private static Vector4 ConvertPoseToPoseGss(Pose2D pose)
        {
            return new Vector4(pose.X, pose.Y, Math.Cos(pose.Heading), Math.Sin(pose.Heading));
        }

        /// <summary>
        /// Convert pose from the global state space (GSS).
        /// </summary>
        /// <param name="poseGss">Pose to be converted.</param>
        /// <returns>Pose with heading represented as usual.</returns>
        private static Pose2D ConvertPoseGssToPose(Vector4 poseGss)
        {
            return new Pose2D(poseGss.X, poseGss.Y, Math.Atan2(poseGss.W, poseGss.Z));
        }

        /// <summary>
        /// Convert an array of poses to the global state space (GSS) by representing the heading as its sine and cosine.
        /// </summary>
        /// <param name="poses">Poses to be converted.</param>
        /// <returns>Poses with heading represented as its sine and cosine.</returns>
        private static Vector4[] ConvertPoseToPoseGssArray(Pose2D[] poses)
        {
            Vector4[] posesGss = new Vector4[poses.Length];
            for (int i = 0; i < poses.Length; i++)
            {
                posesGss[i] = ConvertPoseToPoseGss(poses[i]);
            }

            return posesGss;
        }

        /// <summary>
        /// Convert an array of poses from the global state space (GSS).
        /// </summary>
        /// <param name="posesGss">Poses to be converted.</param>
        /// <returns>Poses with heading represented as usual.</returns>
        private static Pose2D[] ConvertPoseGssToPoseArray(Vector4[] posesGss)
        {
            Pose2D[] poses = new Pose2D[posesGss.Length];
            for (int i = 0; i < posesGss.Length; i++)
            {
                poses[i] = ConvertPoseGssToPose(posesGss[i]);
            }

            return poses;
        }

        /// <summary>
        /// Compute the difference between two poses (not necessarily consecutive).
        /// The difference is in a global state space (GSS).
        /// </summary>
        /// <param name="pose1">first pose $\bfp_a$, according to the notation in the paper.</param>
        /// <param name="pose2">second pose $\bfp_b$.</param>
        /// <returns>Difference between the poses, with heading represented by its sine and cosine.</returns>
        private static Vector4 ConvertPosePairToPoseDiffGss(Pose2D pose1, Pose2D pose2)
        {
            return ConvertPoseToPoseGss(pose2) - ConvertPoseToPoseGss(pose1);
        }

        /// <summary>
        /// Converts two poses (not necessarily consecutive) into the graph edge between them.
        /// </summary>
        /// <param name="a">index $a$ of the first pose.</param>
        /// <param name="pose1">first pose $\bfp_a$, according to the notation in the paper.</param>
        /// <param name="b">index $b$ of the second pose.</param>
        /// <param name="pose2">second pose $\bfp_b$.</param>
        /// <returns>edge between the poses, that is, pose $b$ in frame $a$, $^a\bfp_b$.</returns>
        private static RelativePoseEdge ConvertPosePairToEdge(int a, Pose2D pose1, int b, Pose2D pose2)
        {
            Pose2D relativePose = ConvertPosePairToRelativePose(pose1, pose2);
            return new RelativePoseEdge(a, b, relativePose);
        }

        /// <summary>
        /// Converts two poses (not necessarily consecutive) into the graph edge between them.
        /// Edge is in the global state space (GSS).
        /// </summary>
        /// <param name="a">index $a$ of the first pose.</param>
        /// <param name="pose1">first pose $\bfp_a$, according to the notation in the paper.</param>
        /// <param name="b">index $b$ of the second pose.</param>
        /// <param name="pose2">second pose $\bfp_b$.</param>
        /// <returns>edge between the poses, that is, pose $b$ in frame $a$, $^a\bfp_b$.</returns>
        private static PoseEdgeGss ConvertPosePairToEdgeGss(int a, Pose2D pose1, int b, Pose2D pose2)
        {
            Vector4 poseDiff = ConvertPosePairToPoseDiffGss(pose1, pose2);
            return new PoseEdgeGss(a, b, poseDiff);
        }

        /// <summary>
        /// Holds an edge in the graph used by GraphOptimization.
        /// Note that operators ==, !=, and Equals() are not provided, and the default implementations will be very slow.
        /// </summary>
        internal struct RelativePoseEdge
        {
            /// <summary>
            /// Index of the first node.
            /// </summary>
            public int A;

            /// <summary>
            /// Index of the second node.
            /// </summary>
            public int B;

            /// <summary>
            /// Relative transformation between the two nodes.
            /// </summary>
            public Pose2D Delta;

            /// <summary>
            /// Initializes a new instance of the RelativePoseEdge struct.
            /// </summary>
            /// <param name="a">index of first node.</param>
            /// <param name="b">index of second node.</param>
            /// <param name="delta">relative transformation between nodes.</param>
            public RelativePoseEdge(int a, int b, Pose2D delta)
            {
                this.A = a;
                this.B = b;
                this.Delta = delta;
            }
        }

        /// <summary>
        /// Holds an edge in the graph used by GraphOptimization.
        /// Like RelativePoseEdge but in the global state space (GSS), and heading is stored as its cosine and sine.
        /// Note that operators ==, !=, and Equals() are not provided, and the default implementations will be very slow.
        /// </summary>
        internal struct PoseEdgeGss
        {
            /// <summary>
            /// Index of the first node.
            /// </summary>
            public int A;

            /// <summary>
            /// Index of the second node.
            /// </summary>
            public int B;

            /// <summary>
            /// Difference between the two nodes.
            /// </summary>
            public Vector4 Delta;

            /// <summary>
            /// Initializes a new instance of the <see cref="PoseEdgeGss"/> struct.
            /// </summary>
            /// <param name="a">index of first node.</param>
            /// <param name="b">index of second node.</param>
            /// <param name="delta">relative transformation between nodes.</param>
            public PoseEdgeGss(int a, int b, Vector4 delta)
            {
                this.A = a;
                this.B = b;
                this.Delta = delta;
            }
        }
    }
}
